#include "arm_controller.h"
//#include "device/touchdevice.h"
#include "main_controller.h"
#include <communication/typetransfer.h>
#include "ctype.h"
using namespace Eigen;
using namespace std;
using namespace arm;
const double ArmController::small_m2s_mapping_coeff=1.0;
const double ArmController::large_m2s_mapping_coeff=10.0;

ArmController::ArmController(MainController *main_controller, const SlaveIndex slave_index)
    :BaseController (main_controller,slave_index),joint_num_(7)
{
    arm_end_pos_target_=Vector3d::Zero();
    arm_end_quat_target_=Eigen::Quaterniond::Identity();
    arm_end_v_target_=Eigen::Vector3d::Zero();
    arm_end_w_target_=Eigen::Vector3d::Zero();

    arm_end_pos_cur_=Eigen::Vector3d::Zero();
    //TO DELETE
    arm_end_pos_cur_<<0,-0.25,-0.4;
    //
    arm_end_quat_cur_=Quaterniond::Identity();
    arm_end_v_cur_=Eigen::Vector3d::Zero();
    arm_end_w_cur_=Eigen::Vector3d::Zero();
    arm_end_wrench_cur_=VectorXd::Zero(6);

    hand_joint_pos_cur_=0;
    hand_joint_force_cur_=0;
    hand_joint_pos_target_=0;


    SetM2SArmMappingCoeffLarge();
    s2m_arm_force_mapping_coeff=0.1;
    m2s_gripper_pos_mapping_coeff=1;
    s2m_gripper_force_mapping_coeff=0.1;

    updated_attr_=0;

//    master_origin_pos_=Eigen::Vector3d(0,0,0);
//    master_temp_origin_pos_=Eigen::Vector3d(master_origin_pos_);
//    master_target_pos_=Eigen::Vector3d(master_origin_pos_);
//    master_cur_pos_=Eigen::Vector3d(0,0,0);

//    stop_=false;

//    m2s_transf_=Matrix3d::Identity();
//    s2m_transf_=Matrix3d::Identity();

//    master_cur_pos_=Eigen::Vector3d(0,0,0);
    fsm_.reset(new FSM(this));
    //slave_.reset(new SlaveArm(slave_index,7));

    /* 7 axis*/
    tempsb_t_mb_<<-1,0,0,0,
                    0,-1,0,0,
                    0,0,1,0,
                    0,0,0,1;
    mb_t_tempsb_=tempsb_t_mb_.inverse();
    mh_t_vsh_<<0,0,-1,0,
                1,0,0,0,
                0,-1,0,0,
                0,0,0,1;
    vsh_t_mh_=mh_t_vsh_.inverse();
    sb_t_tempsb_<<1,0,0,0,
                0,1,0,0,
                0,0,1,0,
                0,0,0,1;

    slave_default_base_origin_<<0.4,-0.25,-0.4;

    /*
     * 6 axis

    tempsb_t_mb_<<1,0,0,0,
                0,1,0,0,
                0,0,1,0,
                0,0,0,1;
    mb_t_tempsb_=tempsb_t_mb_.inverse();
    mh_t_vsh_<<1,0,0,0,
            0,0,-1,0,
            0,1,0,0,
            0,0,0,1;
    vsh_t_mh_=mh_t_vsh_.inverse();
    sb_t_tempsb_<<1,0,0,0,
                0,1,0,0,
                0,0,1,0,
                0,0,0,1;
     slave_default_base_origin_<<-0.5,-0.12,0.6;
     */


    ResetOriginPos();//set the translate part of the sb_t_tempsb
}

ArmController::~ArmController()
{
//    stop_=true;
}


//bool ArmController::ParseFrameData(const u_char *data, const size_t len)
//{
//    return ParseUserData(data,len);
//}

//bool ArmController::PackFrameData(u_char *data,size_t &len)
//{
//    size_t usr_data_length=0;
//    bool ret = PackUsrData(data+11,usr_data_length);//usr data length without ending two bytes crc

//    if(!ret)
//    {
//        len=0;
//        return false;
//    }
//    data[0]=SunlinkProtocal::kSunlinkHeader1;
//    data[1]=SunlinkProtocal::kSunlinkHeader2;
//    data[2]=SunlinkProtocal::kSunlinkVersion;

//    TypeTransfer<u_int16_t>::Type2ByteArray(data+3,static_cast<u_int16_t>(usr_data_length+2+6));//pure_usr_data+len+data_type+target

//    TypeTransfer<u_int16_t>::Type2ByteArray(data+5,static_cast<u_int16_t>(RC2Robot_Control));
//    TypeTransfer<int32_t>::Type2ByteArray(data+7,static_cast<int32_t>(main_controller_->GetTargetRobotIndex()));

//    u_int16_t crc=CalCRC16(data,usr_data_length+11);
//    TypeTransfer<u_int16_t>::Type2ByteArray(data+11+usr_data_length,crc);//crc
//    len=11+usr_data_length+2;                               //head+usr_data+crc

//    //main_controller_->SendFrameData(data,len);
//    return true;
//}

/*
 *  const int joint_num_;
 *  Eigen::VectorXd arm_joint_pos_cur_,arm_joint_vel_cur_;
 *  Eigen::VectorXd arm_end_wrench_cur_;
 *  double finger_joint_pos_cur_,finger_joint_vel_cur_,finger_joint_force_cur_;
 */
bool ArmController::ParseUsrData(const u_char *buffer, const size_t buffer_len)
{
    if(buffer_len<=2)
        return false;
    size_t index=0;
//    u_char part=0;
//    TypeTransfer<u_char>::ByteArray2Type(part,buffer+index);
    index+=sizeof (u_char);
    u_char attr=0;
    TypeTransfer<u_char>::ByteArray2Type(attr,buffer+index);
    index+=sizeof (attr);
    SocketDataType data;

    const size_t data_size=sizeof (SocketDataType);
    //enum ArmAttrType{kEndPos=0x01,kEndQuat=0x02,kEndLinearVel=0x04,kEndAngularVel=0x08,kEndForce=0x10,kJointPos=0x20,kJointVel=0x40,kJointForce=0x80};
    size_t total_len=(attr&Slave2Master::ArmAttrType::kArmEndPos)?3:0
                     +(attr&Slave2Master::ArmAttrType::kArmEndQuat)?4:0
                     +(attr&Slave2Master::ArmAttrType::kArmEndLinearVel)?3:0
                     +(attr&Slave2Master::ArmAttrType::kArmEndAngularVel)?3:0
                     +(attr&Slave2Master::ArmAttrType::kArmEndWrench)?6:0
                     +(attr&Slave2Master::ArmAttrType::kHandJointPos)?1:0
                     +(attr&Slave2Master::ArmAttrType::kHandJointForce)?1:0;
    total_len=total_len*sizeof (SocketDataType);
    if(buffer_len!=(1+1+total_len+2))//part+attr+data+crc
    {
        cout<<"wrong frame data: length doesn't match"<<endl;
        return false;
    }
    if(attr&Slave2Master::ArmAttrType::kArmEndPos)
    {
        cout<<"end pos updated"<<endl;
        for (int i=0;i<3;i++) {
            TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
            arm_end_pos_cur_[i]=static_cast<double>(data);
            index+=data_size;
        }
    }
    if(attr&Slave2Master::ArmAttrType::kArmEndQuat)
    {
        cout<<"end quat updated"<<endl;
        TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
        arm_end_quat_cur_.w()=static_cast<double>(data);
        index+=data_size;
        TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
        arm_end_quat_cur_.x()=static_cast<double>(data);
        index+=data_size;
        TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
        arm_end_quat_cur_.y()=static_cast<double>(data);
        index+=data_size;
        TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
        arm_end_quat_cur_.z()=static_cast<double>(data);
        index+=data_size;
    }
    if(attr&Slave2Master::ArmAttrType::kArmEndLinearVel)
    {
        for (int i=0;i<3;i++) {
            TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
            arm_end_v_cur_[i]=static_cast<double>(data);
            index+=data_size;
        }
    }
    if(attr&Slave2Master::ArmAttrType::kArmEndAngularVel)
    {
        for (int i=0;i<3;i++) {
            TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
            arm_end_w_cur_[i]=static_cast<double>(data);
            index+=data_size;
        }
    }
    if(attr&Slave2Master::ArmAttrType::kArmEndWrench)
    {
        for (int i=0;i<6;i++) {
            TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
            arm_end_wrench_cur_[i]=static_cast<double>(data);
            index+=data_size;
        }
    }
    if(attr&Slave2Master::ArmAttrType::kHandJointForce)
    {
        TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
        hand_joint_pos_cur_=static_cast<double>(data);
        index+=data_size;
    }
    if(attr&Slave2Master::ArmAttrType::kHandJointForce)
    {
        TypeTransfer<SocketDataType>::ByteArray2Type(data,buffer+index);
        hand_joint_force_cur_=static_cast<double>(data);
        index+=data_size;
    }

    return true;
}

/*
 *  Eigen::Vector3d arm_end_pos_target_;
 *  Eigen::Quaterniond arm_end_quat_target_;
 *  Eigen::Vector3d arm_end_v_target_;
 *  Eigen::Vector3d arm_end_w_target_;
 *
 *
 *  double finger_joint_pos_target_;
 */

bool ArmController::PackUsrData(u_char *buffer,size_t &buffer_len)
{
    if(updated_attr_==0x00)
    {
        buffer_len=0;
        return false;
    }
    //u_char type=GetType();

    size_t index=0;
    TypeTransfer<u_char>::Type2ByteArray(buffer+index,static_cast<u_char>(slave_target_index_));
    index+=sizeof (u_char);
    TypeTransfer<u_char>::Type2ByteArray(buffer+index,updated_attr_);
    index+=sizeof (updated_attr_);

    const size_t data_size=sizeof (SocketDataType);
    if (updated_attr_&Master2Slave::kArmEndPos)
    {
        for (int i=0;i<3;i++)
        {
            TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_pos_target_[i]));
            index+=data_size;
        }
    }
    if (updated_attr_&Master2Slave::kArmEndQuat)
    {
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_quat_target_.w()));
        index += data_size;
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_quat_target_.x()));
        index += data_size;
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_quat_target_.y()));
        index += data_size;
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_quat_target_.z()));
        index += data_size;
    }
    if (updated_attr_&Master2Slave::kArmEndLinearVel)
    {
        for (int i=0;i<3;i++) {
            TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_v_target_[i]));
            index += data_size;
        }
    }
    if (updated_attr_&Master2Slave::kArmEndAngularVel)
    {
        for (int i=0;i<3;i++) {
            TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_w_target_[i]));
            index += data_size;
        }
    }
    if (updated_attr_&Master2Slave::kArmEndWrench)
    {
        for (int i=0;i<6;i++) {
            TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(arm_end_wrench_cur_[i]));
            index += data_size;
        }
    }
    if (updated_attr_&Master2Slave::kHandJointPos)
    {
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(hand_joint_pos_target_));
        index += data_size;
    }
    if(updated_attr_&Master2Slave::kHandJointForce)
    {
        TypeTransfer<SocketDataType>::Type2ByteArray(buffer+index,static_cast<SocketDataType>(hand_joint_force_cur_));
        index += data_size;
    }


#ifdef VERBOSE
{
    if (updated_attr_&Master2Slave::kArmEndPos)
        cout<<"target_end_pos: "<<arm_end_pos_target_[0]<<","<<arm_end_pos_target_[1]<<","<<arm_end_pos_target_[2]<<endl;
    if (updated_attr_&Master2Slave::kArmEndQuat)
        cout<<"target_end_quat: "<<arm_end_quat_target_.w()<<" "<<arm_end_quat_target_.x()<<" "<<arm_end_quat_target_.y()<<" "<<arm_end_quat_target_.z()<<" "<<endl;
    if (updated_attr_&Master2Slave::kArmEndLinearVel)
        cout<<"target_end_v: "<<arm_end_v_target_[0]<<","<<arm_end_v_target_[1]<<","<<arm_end_v_target_[2]<<endl;
    if (updated_attr_&Master2Slave::kArmEndAngularVel)
        cout<<"target_end_w: "<<arm_end_w_target_[0]<<","<<arm_end_w_target_[1]<<","<<arm_end_w_target_[2]<<endl;

    if (updated_attr_&Master2Slave::kArmEndWrench)
    {
        cout<<"arm_end_wrench_cur: ";
        for (size_t i=0;i<6;i++)
            cout<<arm_end_wrench_cur_[i]<<",";
        cout<<endl;
    }

    if (updated_attr_&Master2Slave::kHandPos)
        cout<<"hand_joint_pos_target: "<<hand_joint_pos_target_<<endl;
    if (updated_attr_&Master2Slave::kHandForce)
        cout<<"hand_joint_force_cur: "<<hand_joint_force_cur_<<endl;
}
#endif

    ClearTargetStatus();

    if(index<=2)
    {
        buffer_len=0;
        return false;
    }
    else
    {
//        cout<<"send msg:";
//        for (int i=0;i<index;i++)
//        {
//            cout<<(int)buffer[i]<<" ";
//        }
//        cout<<endl;
        buffer_len=index;
        return true;
    }
    //return index;
}



ErrorCode ArmController::Update()
{
    if(master_device_index_==nullptr || *master_device_index_<0)
        return kOkay;
    fsm_->Update();
    return kOkay;
}

void ArmController::SetOriginPos()
{
    sb_t_tempsb_.topRightCorner(3,1)=GetArmCurEndPos();
    tempsb_t_sb_=sb_t_tempsb_.inverse();

}

void ArmController::ResetOriginPos()
{
    sb_t_tempsb_.topRightCorner(3,1)=slave_default_base_origin_;
    tempsb_t_sb_=sb_t_tempsb_.inverse();

}

void ArmController::SendTarget2Slave(const Eigen::Matrix4d &master_arm_cur_pose, const double master_handjoint_cur_pos)
{
    if(*master_device_index_<0)
    {
        ClearTargetStatus();
        return;
    }
    SetArmTargetEndPose(Master2Slave(master_arm_cur_pose));
    SetHandTargetJointPos(master_handjoint_cur_pos*m2s_gripper_pos_mapping_coeff);
}

Vector3d ArmController::GetArmForce(const Eigen::Matrix3d &mb2mh) const
{
    //return mb2mh*mh2sh.block(0,0,3,3)*(slave_->GetArmCurEndWrench().block(0,0,3,1)*s2m_arm_force_mapping_coeff);

    return Vector3d::Zero();
}

double ArmController::GetHandJointForce(const double master_handjoint_cur_pos) const
{

    return s2m_gripper_force_mapping_coeff*(GetHandCurJointPos()-master_handjoint_cur_pos);

}


Eigen::Matrix4d ArmController::Master2Slave(const Eigen::Matrix4d &mb_t_mh)
{
   Eigen::Matrix4d m=mb_t_mh*mh_t_vsh_;
   m(0,3)*=m2s_arm_pos_mapping_coeff;
   m(1,3)*=m2s_arm_pos_mapping_coeff;
   m(2,3)*=m2s_arm_pos_mapping_coeff;
   return sb_t_tempsb_*tempsb_t_mb_*m;
}

Eigen::Matrix4d ArmController::Slave2Master(const Eigen::Matrix4d &sb_t_sh)
{

   Eigen::Matrix4d m=tempsb_t_sb_*sb_t_sh;
   m(0,3)/=m2s_arm_pos_mapping_coeff;
   m(1,3)/=m2s_arm_pos_mapping_coeff;
   m(2,3)/=m2s_arm_pos_mapping_coeff;
   return mb_t_tempsb_*m*vsh_t_mh_;
}






